
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>


#include "commKTask.h"

#include "FreeRTOS.h"
#include "task.h"

#include "nxbot.h"

extern xTaskHandle commHandle;
extern xTaskHandle soundHandle;

static void commTask(void *pvParameters);

void executeCommTask(general *pvParameters)
{
	
	xTaskCreate(commTask,(const signed portCHAR * const)"commTask1",commTask_stackSize,pvParameters,tskIDLE_PRIORITY + 3,&commHandle);

}
	
static void commTask(void *pvParameters)
{
	unsigned int freq;
	portTickType actualTick;
	
	general *globals;
	globals=(general *)pvParameters;
	int temp=-1;
	stream commStream;	
	stream* streamPtr;
	streamPtr = &commStream;

	
	
	for(;;){
		
		//aquire the frequency for the task		
		freq=globals->gTimes.intervalComm;
		
		while(1){
			
			if(!streamRx(&commStream,COMM_PORT)){
				
					actualTick = xTaskGetTickCount();
					vTaskDelayUntil(&actualTick,(portTickType)freq);
			}
			else	
				break;
			
		}
		
		switch(commStream.command){
	
			//Command: B
			case GET_SW_VERSION:
			
				sprintf(streamPtr->data,",%d.%d",(int)SW_VERSION_H,(int)SW_VERSION_L);
				
				commStream.command+=32;				
				
				streamTx(&commStream,COMM_PORT);
			break;
			
			//Command: E
			case GET_SPEED:
				
				sprintf(streamPtr->data,",%d,%d",globals->gVels.velLeftAct,globals->gVels.velRightAct);

				commStream.command+=32;
								
				streamTx(&commStream,COMM_PORT);

			break;
			
			//Command: D
			case SET_SPEED:

				temp=sscanf(streamPtr->data,",%d,%d",&globals->gVels.velLeftRef,&globals->gVels.velRightRef);
				
				if(temp<2){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;

					streamTx(&commStream,COMM_PORT);
					
					break;
				}
				
				commStream.data[0]='\0'; //No data should be returned!
				commStream.command+=32;
				
				streamTx(&commStream,COMM_PORT);
			break;
			
			//Command: H
			case GET_POSITION:
			
				sprintf(streamPtr->data,",%f,%f,%f",(double)globals->gRobotState.posXAct,(double)globals->gRobotState.posYAct,(double)globals->gRobotState.orientation);
				
				commStream.command+=32;				

				streamTx(&commStream,COMM_PORT);

			break;
			
			//Command: S
			case GET_ENCODERS:
			
				sprintf(streamPtr->data,",%ld,%ld",globals->gEncod.posLeft,globals->gEncod.posRight);
				
				commStream.command+=32;				

				streamTx(&commStream,COMM_PORT);

			break;
			
			//Command: O
			case SET_ENCODERS:
			
				temp=sscanf(streamPtr->data,",%ld,%ld",&globals->gEncod.posLeft,&globals->gEncod.posRight);
				
				//reset values
				globals->gEncod.lastPosLeft = globals->gEncod.posLeft;
				globals->gEncod.lastPosRight = globals->gEncod.posRight;
				
				if(temp<2){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					streamTx(&commStream,COMM_PORT);
					
					break;
				}
				
				//reset odometers
				encoders_setValues(0,0);
				
				commStream.data[0]='\0'; //No data should be returned!
				commStream.command+=32;	
				
				streamTx(&commStream,COMM_PORT);

			break;
			
			//Command: G
			case SET_POSITION:
			
				//temp=sscanf(streamPtr->data,",%ld,%ld",&globals->gRobotState.posXAct,&globals->gRobotState.posYAct);
				temp=sscanf(streamPtr->data,",%f,%f,%f",&globals->gRobotState.posXAct,&globals->gRobotState.posYAct,&globals->gRobotState.orientation);
				
				if(temp<3){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					streamTx(&commStream,COMM_PORT);
					
					break;
				}
				//Yaw is a copy of orientation. It has to be inverted, thats why the minus. See motorsTask where orientation = -yaw.
				globals->gRobotState.yaw = - (globals->gRobotState.orientation);
				
				//reset odometers
				encoders_setValues(0,0);
				
				//reset values of encoders
				globals->gEncod.posLeft = 0;
				globals->gEncod.posRight = 0;
				globals->gEncod.lastPosLeft = 0;
				globals->gEncod.lastPosRight = 0;
				
				
				commStream.data[0]='\0'; //No data should be returned!
				commStream.command+=32;	
				
				streamTx(&commStream,COMM_PORT);

			break;
			
			//Command: N
			case GET_IRSENSOR:
			
				sprintf(streamPtr->data,",%d",globals->gSensors.sensorsIR);
				
				commStream.command+=32;				
				
				streamTx(&commStream,COMM_PORT);
			break;
			
			//Command: M
			case GET_BUMPSENSOR:
			
				sprintf(streamPtr->data,",%d",globals->gSensors.bumpers);
				
				commStream.command+=32;				
				
				streamTx(&commStream,COMM_PORT);
			break;
			
			//Command: Q
			case GET_BATT_STATE:
			
				sprintf(streamPtr->data,",%d,%d",globals->gSensors.batt.battState,globals->gSensors.batt.battVoltage);
				
				commStream.command+=32;				
				
				streamTx(&commStream,COMM_PORT);
			break;
			
			//Command: P
			case SET_KP:
				
				sscanf(streamPtr->data,",%d",&globals->gPiParam.Kp_int);
				
				if(temp<1){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					streamTx(&commStream,COMM_PORT);
					
					break;
				}
				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				streamTx(&commStream,COMM_PORT);
			break;
			
			//Command: I
			case SET_KI:
				
				sscanf(streamPtr->data,",%d",&globals->gPiParam.Ki_int);			
				
				if(temp<1){
					temp=-1;
					commStream.command=COMMUNICATION_ERROR;
				
					streamTx(&commStream,COMM_PORT);
					
					break;
				}
				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				streamTx(&commStream,COMM_PORT);
			break;
			
			//Command: R
			case SET_SOUND_JINGLE:
				
				sscanf(streamPtr->data,",%d",&globals->soundJingle);			
				

				commStream.command+=32;	
				commStream.data[0]='\0'; //No data should be returned!
				
				streamTx(&commStream,COMM_PORT);
				vTaskResume( soundHandle );
			break;
			
			default:

				commStream.command=COMMUNICATION_ERROR;
				
				streamTx(&commStream,COMM_PORT);
			break;
		}
		
	}
}

